/***************************************************************************
 *  Filename: groundscommunicator.cpp
 *  Class: GroundSCommunicator
 *
 *  Copyright 2012  Jacco van der Spek
 *  <jaccovdspek@gmail.com>
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *
 ****************************************************************************/

/*
 *
 * This file is part of %ProjectName%.
 *
 *    %ProjectName% is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    %ProjectName% is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with %ProjectName%.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "groundscommunicator.h"
#include <QDebug>

GroundSCommunicator::GroundSCommunicator(QString ttyPort, QObject *parent) :
    QObject(parent), communicator(true, parent)
{
    communicator.setupPort(ttyPort, BAUD115200, DATA_8, PAR_NONE, FLOW_HARDWARE, STOP_2);
    connect(&communicator, SIGNAL(byteAvailable()), this, SLOT(receiveByte()));
}

void GroundSCommunicator::receiveByte(){
    dataReceived.clear();
    dataReceived.append(QByteArray::fromRawData(communicator.receiveRawFromPort(1), 1));
    if(*reinterpret_cast<short*>(dataReceived.data())==252){
        dataReceived.append(QByteArray::fromRawData(communicator.receiveRawFromPort(1), 1));
        short cmd = *reinterpret_cast<short*>(dataReceived.mid(1,1).data());
        switch(cmd){
        case 0:
            qWarning() << "[GroundSCommunicator] Command to hover received";
            emit cmdHover();
            break;
        case 31:
            qWarning() << "[GroundSCommunicator] Command to track received";
            emit cmdTrack();
            break;
        case 227:
            qWarning() << "[GroundSCommunicator] Command to return received";
            emit cmdReturn();
            break;
        case 252:
            qWarning() << "[GroundSCommunicator] Command to fly received";
            emit cmdFlyTo();
            break;
        default:
            qWarning() << "[GroundSCommunicator] Command received doesn't seem to be valid";
            qWarning() << "[GroundSCommunicator] Received data is:" << dataReceived.toHex();
        }
    }
}

void GroundSCommunicator::sendFlightInfo(FlightData data){
    qDebug() << "[GroundSCommunicator] Encapsulating flight data";
    QByteArray toSend;
    quint8 header = HEADER_UAVINFO;
    toSend.append(QByteArray::fromRawData((char*)&header, 1));
    float lat = data.getGPSData().getLatitude();
    float lon = data.getGPSData().getLongitude();
    float alt = data.getAltitude();
    float vel = data.getGPSData().getVelocity();
    quint16 bat = data.getBattery();
    toSend.append(QByteArray::fromRawData((char*)&lat, 4));
    toSend.append(QByteArray::fromRawData((char*)&lon, 4));
    toSend.append(QByteArray::fromRawData((char*)&alt, 4));
    toSend.append(QByteArray::fromRawData((char*)&vel, 4));
    toSend.append(QByteArray::fromRawData((char*)&bat, 2));
    communicator.sendToPort(toSend);
    qDebug() << "[GroundSCommunicator] Encapsulated and sent data: " << toSend.toHex();
}

void GroundSCommunicator::sendTargetInfo(GPSData data){
    qDebug() << "[GroundSCommunicator] Encapsulating target data";
    QByteArray toSend;
    quint8 header = HEADER_CARGPS;
    toSend.append(QByteArray::fromRawData((char*)&header, 1));
    float lat = data.getLatitude();
    float lon = data.getLongitude();
    float vel = data.getVelocity();
    toSend.append(QByteArray::fromRawData((char*)&lat, 4));
    toSend.append(QByteArray::fromRawData((char*)&lon, 4));
    toSend.append(QByteArray::fromRawData((char*)&vel, 4));
    communicator.sendToPort(toSend);
    qDebug() << "[GroundSCommunicator] Encapsulated and sent data: " << toSend.toHex();
}

void GroundSCommunicator::sendImage(QString filename){
    qDebug() << "[GroundSCommunicator] Encapsulating image data";
    QByteArray toSend;
    quint8 header = HEADER_IMG;
    QFile imageFile(filename);
    imageFile.open(QIODevice::ReadWrite);
    QByteArray imageData = imageFile.readAll();
    imageFile.close();
    qDebug() << "[GroundSCommunicator] Image file closed";
    int size = imageData.size();
    toSend.append(QByteArray::fromRawData((char*)&header, 1));
    toSend.append(QByteArray::fromRawData((char*)&size, 4));
    toSend.append(imageData);
    communicator.sendToPort(toSend);
    qDebug() << "[GroundSCommunicator] Encapsulated and sent picture data size" << toSend.size();
}

void GroundSCommunicator::sendSteering(quint8 data){
    QByteArray toSend;
    quint8 header = HEADER_STEER;
    toSend.append(QByteArray::fromRawData((char*)&header, 1));
    toSend.append(QByteArray::fromRawData((char*)&data, 1));
    communicator.sendToPort(toSend);
    qDebug() << "[GroundSCommunicator] Encapsulated and sent data: " << toSend.toHex();
}
